/*
 * Heuristics.c
 *
 *  Created on: 24/apr/2013
 *      Authors: Ialenti Andrea, Marangon Fabio
 */

#include "../headers/Heuristic.h"
#include "../headers/OneTree.h"
#include "../headers/Printer.h"
#include "../headers/StatusCodes.h"
#include "../headers/Utility.h"

#include <stdlib.h>
#include <string.h>

//	Function used to perform the 2-opt swapping move.
Status do2OptSwap(OneTree *t, const Graph *g, int i, int j);

//	Function used to restore the tour after the swap.
Status restoreTour(OneTree *t, int i, int j);

//	Function used to check if two edges are connected one-another.
bool connected(const OneTree *t, int i, int j);

//	Execute a random 3-opt on the three edges 'i', 'j', 'k'.
bool do3OptSwap(OneTree *t, const Graph *g, int i, int j, int k);

Status findBestNN_2Opt_3Opt(const Config *cfg, const Graph *g, OneTree *tour, int *swapCnt, bool use3opt, bool verbose) {
	char *funcName = initCharArray(cfg->NAME_LEN);
	sprintf(funcName, "findBestNN_2Opt_3Opt");

	int i, j, k, startingNode;
	int *flag = initIntArray(g->size);
	int *succ = initIntArray(g->size);
	int idx = 0;
	double min = cfg->INF;
	time_t t1, t2;

	OneTree tmp;
	bool init = initOneTree(cfg, &tmp, g->size);
	if (!init) {
		fprintf(stdout, "***'%s': 1-tree memory initialization error.\n", funcName);
		freeCharArray(funcName);
		disposeOneTree(&tmp);
		freeIntArray(flag);
		freeIntArray(succ);
		return ERR_MEM_ALLOC;
	}

	//	Start from each different node, and keep the best tour.
	for (startingNode = 0, tour->tc = cfg->INF, time(&t1); startingNode < g->size; startingNode++) {

		//	Re-initialize flags and predecessors.
		for (j = 0; j < g->size; j++)
			flag[j] = succ[j] = 0;
		j = startingNode;
		flag[j] = 1;

		//	Find the nearest neighbor tour.
		for (i = 0; i < g->size; i++) {

			for (k = 0, min = cfg->INF; k < g->size; k++)
				if (flag[k] == 0 && g->edges[j][k].cost < min && !(g->edges[j][k].cost > -cfg->EPS && g->edges[j][k].cost < cfg->EPS)) {
					min = g->edges[j][k].cost;
					idx = k;
				}

			flag[idx] = 1;
			succ[j] = idx;
			j = idx;
		}
		succ[idx] = startingNode;

		//	Build the 1-tree representing the tour.
		resetCosts(&tmp);
		tmp.v0 = startingNode;
		tmp.i1 = succ[startingNode];
		tmp.i2 = idx;

		for (i = 0; i < tmp.size; i++) {

			//	Copy the nodes.
			cpNode(&g->nodes[i], &tmp.nodes[i]);
			cpNode(&g->nodes[succ[i]], &tmp.nodes[succ[i]]);

			//	Set the edges nodes and costs.
			tmp.edges[i].start = &tmp.nodes[i];
			tmp.edges[i].end = &tmp.nodes[succ[i]];
			tmp.edges[i].cost = g->edges[i][succ[i]].cost;

			//	Link the edges.
			tmp.edges[i].next = &tmp.edges[succ[i]];
			tmp.edges[succ[i]].prev = &tmp.edges[i];

			//	Update the tour cost.
			tmp.tc += tmp.edges[i].cost;
			tmp.totalCost += g->costs_origin[i][succ[i]];
		}

		//	Run 2-opt to optimize the nearest neighbor solution.
		run2Opt(cfg, g, &tmp, swapCnt);

		//	Save the current solution if it is better than the previous one.
		if (tour->tc - tmp.tc > cfg->EPS) {
			if (verbose)
				fprintf(stdout, "\t\t'%s': 2-opt improved NN solution from node n%d! Lagrangian cost = %0.2f. Original cost = %0.0f.\n", funcName, startingNode, tmp.tc, tmp.totalCost);
			cpTree(&tmp, tour);
		}

		//	Try to further improve the solution with some random 3-opt swaps.
		if (use3opt) {
			for (i = 0; i < cfg->IT_3OPT; i++) {

				//	Randomly choose 3 different edges for the 3-opt swap.
				int e1 = -1, e2 = -1, e3 = -1;
				e1 = rand() % g->size;
				while (e2 == e1 || e2 == -1)
					e2  = rand() % g->size;
				while (e3 == e2 || e3 == e1 || e3 == -1)
					e3  = rand() % g->size;

				do3OptSwap(&tmp, g, e1, e2, e3);
				run2Opt(cfg, g, &tmp, swapCnt);

				//	Update the 'tour' if the solution is improved.
				if (tour->tc - tmp.tc > cfg->EPS) {
					if (verbose)
						fprintf(stdout, "\t\t\t'%s': 3-opt -> 2-opt improved the NN -> 2-opt solution from node n%d! Lagrangian cost = %0.2f. Original cost = %0.0f.\n", funcName, startingNode, tmp.tc, tmp.totalCost);
					cpTree(&tmp, tour);
				}
			}
		}

		if (verbose && startingNode % 25 == 0 && startingNode != 0) {
			time(&t2);
			if (verbose) {
				fprintf(stdout, "\t\t'%s': search from node n%d completed. Elapsed time: %0.2f second(s) (%0.2f second(s) normalized time), continuing. . .\n", funcName, startingNode, difftime(t2, t1), difftime(t2, t1) * cfg->NORM_TIME);
				fflush(stdout);
			}
		}
	}
	strcpy(tour->name, g->name);

	//	Free memory.
	freeCharArray(funcName);
	disposeOneTree(&tmp);
	freeIntArray(flag);
	freeIntArray(succ);
	return SUCCESS;
}

Status run2Opt(const Config *cfg, const Graph *g, OneTree *tour, int *swapCnt) {
	int i, j;
	double bestCost = tour->tc;

	for (i = 0; i < tour->size - 1; i++)
		for (j = i + 1; j < tour->size; j++)

			//	Skip edges connected one-another.
			if (!connected(tour, i, j)) {

				//	Compute the new tour cost after the swap
				double costNewEdge1 = g->edges[tour->edges[j].end->id][tour->edges[i].end->id].cost;
				double costNewEdge2 = g->edges[tour->edges[j].start->id][tour->edges[i].start->id].cost;
				double newCost = tour->tc - tour->edges[i].cost - tour->edges[j].cost + costNewEdge1 + costNewEdge2;

				//	Swap edges only if the swap improves the old tour cost.
				if(bestCost - newCost > cfg->EPS) {

					do2OptSwap(tour, g, i, j);
					(*swapCnt)++;
					bestCost = tour->tc;

					//	Reset the edge indices to check again all the possible pairs.
					j = tour->size;
					i = -1;
				}
			}
	return SUCCESS;
}

Status do2OptSwap(OneTree *t, const Graph *g, int i, int j) {

	//	Remove the cost of the two edges to be swapped.
	t->tc = t->tc - t->edges[i].cost - t->edges[j].cost;
	t->totalCost = t->totalCost - g->costs_origin[t->edges[i].start->id][t->edges[i].end->id] - g->costs_origin[t->edges[j].start->id][t->edges[j].end->id];

	//	Swap the nodes and links for the two edges and restore the tour.
	Edge Ei = t->edges[i];
	Edge Ej = t->edges[j];

	//	Swap the nodes.
	t->edges[i].start = Ej.end;
	t->edges[j].end = Ei.start;

	//	Swap the links.
	t->edges[i].prev = Ej.next;
	t->edges[j].next = Ei.prev;

	//	Fix the previously connected edges' links.
	Ej.next->prev = &t->edges[i];
	Ei.prev->next = &t->edges[j];

	//	Restore the tour.
	restoreTour(t, i, j);

	//	Set the correct costs for the new edges.
	t->edges[i].cost = g->edges[t->edges[i].start->id][t->edges[i].end->id].cost;
	t->edges[j].cost = g->edges[t->edges[j].start->id][t->edges[j].end->id].cost;

	//	Update the tour cost.
	t->tc += t->edges[i].cost + t->edges[j].cost;
	t->totalCost += g->costs_origin[t->edges[i].start->id][t->edges[i].end->id] + g->costs_origin[t->edges[j].start->id][t->edges[j].end->id];
	return SUCCESS;
}

Status restoreTour(OneTree *t, int i, int j) {

	int sID = t->edges[i].start->id;
	int eID = t->edges[i].end->id;
	Edge *ptr = t->edges[j].next;

	//	Restore the tour.
	do {
		Edge tmp = *ptr;

		//	Swap the two links.
		ptr->next = tmp.prev;
		ptr->prev = tmp.next;

		//	Swap the starting and ending node.
		ptr->start = tmp.end;
		ptr->end = tmp.start;

		ptr = ptr->next;
	} while (ptr->start->id != sID || ptr->end->id != eID);
	return SUCCESS;
}

bool connected(const OneTree *t, int i, int j) {
	return t->edges[i].start->id == t->edges[j].start->id || t->edges[i].start->id == t->edges[j].end->id	|| t->edges[i].end->id == t->edges[j].start->id	|| t->edges[i].end->id == t->edges[j].end->id;
}

bool do3OptSwap(OneTree *t, const Graph *g, int i, int j, int k) {

	//	Set the indices so that they are visited counter-clockwise.
	int jId = t->edges[j].start->id;
	int iId = t->edges[i].start->id;
	int kId = t->edges[k].start->id;
	Edge *ptr = t->edges[i].next;
	do {
		if(ptr->start->id == kId) {
			break;
		}
		if(ptr->start->id == jId) {
			swapInt(&j, &k);
			break;
		}
		ptr = ptr->next;
	} while (ptr->start->id != iId);

	//	Use temporary edges.
	Edge Ei = t->edges[i];
	Edge Ej = t->edges[j];
	Edge Ek = t->edges[k];

	//	Skip edges that have common nodes.
	if (!connected(t, i, j) && !connected(t, i, k) && !connected(t, k, j)) {

		//	Choose the 3-opt swap that minimize the resulting cost.
		double swpMove = t->tc - t->edges[i].cost - t->edges[j].cost - t->edges[k].cost;

		//	Add the new costs.
		swpMove += g->edges[t->edges[i].start->id][t->edges[k].end->id].cost + g->edges[t->edges[j].start->id][t->edges[i].end->id].cost + g->edges[t->edges[k].start->id][t->edges[j].end->id].cost;

		//	Remove the cost of the three edges to be swapped.
		t->tc = t->tc - t->edges[i].cost - t->edges[j].cost - t->edges[k].cost;
		t->totalCost = t->totalCost - g->costs_origin[t->edges[i].start->id][t->edges[i].end->id] - g->costs_origin[t->edges[j].start->id][t->edges[j].end->id] - g->costs_origin[t->edges[k].start->id][t->edges[k].end->id];

		//	Swap the nodes.
		t->edges[i].end = Ek.end;
		t->edges[j].end = Ei.end;
		t->edges[k].end = Ej.end;

		//	Swap the links.
		t->edges[i].next = Ek.next;
		t->edges[j].next = Ei.next;
		t->edges[k].next = Ej.next;

		//	Fix the links.
		Ei.next->prev = &t->edges[j];
		Ej.next->prev = &t->edges[k];
		Ek.next->prev = &t->edges[i];

		//	Set the correct costs for the new edges.
		t->edges[i].cost = g->edges[t->edges[i].start->id][t->edges[i].end->id].cost;
		t->edges[j].cost = g->edges[t->edges[j].start->id][t->edges[j].end->id].cost;
		t->edges[k].cost = g->edges[t->edges[k].start->id][t->edges[k].end->id].cost;

		//	Update the tour cost.
		t->tc += t->edges[i].cost + t->edges[j].cost + t->edges[k].cost;
		t->totalCost += g->costs_origin[t->edges[i].start->id][t->edges[i].end->id] + g->costs_origin[t->edges[j].start->id][t->edges[j].end->id] + g->costs_origin[t->edges[k].start->id][t->edges[k].end->id];
	}
	return true;
}
